#ifndef __SPLINE_HPP__
#define __SPLINE_HPP__

#include<iostream>
#include<cmath>
#include<limits>
#include<stdexcept>
#include<vector>
#include<algorithm>
#include<stdarg.h>
#include<eigen3/Eigen/Dense>
#include<eigen3/Eigen/Core>
#include<eigen3/Eigen/LU>
using namespace std;
using namespace Eigen;

const double epsilon = 1e6*std::numeric_limits<double>::epsilon();

/*
    函数类，用于函数输入
*/
class func{
public:
	virtual double operator () (double x){
		return x;
	}
	virtual double diff(double x){
		return (operator () (x+epsilon)-operator () (x-epsilon))/(2*epsilon);
	}
	virtual double deriv(double x, int p){
		return (operator () (x+epsilon)-operator () (x-epsilon))/(2*epsilon);
	}
};

/*
    样条基类：
        定义基本操作, 默认是恒等映射
*/

class SplineBase{
public:
    bool _is_increasing(vector<double> x){
        for(int i=0;i<x.size()-1;i++){
            if(x[i]>= x[i+1]) return false;
        }
        return true;
    }
    SplineBase(){}
    ~SplineBase(){}
    virtual double operator () (double x){
        return x;
    }
};

/*
    查找函数，全局使用
*/
int _ind_poly(double x, vector<double> _knot){
    if(x<_knot[0]){
        //cout << x << endl;
        return 0;
    }
    else if(x>_knot[_knot.size()-1]){
        //cout <<_knot[_knot.size()-1] <<" " << x << endl;
        return _knot.size()-2;
    }
    int l = 0; int r = _knot.size()-1;
    while(1){
        int mid = (l+r)/2;
        if(x<_knot[mid]) r = mid; 
        else if(x>_knot[mid+1]) l = mid+1;
        else return mid; 
    }
}

void vector_print(vector<double> _input){
    for(int i=0;i<_input.size();i++) cout << _input[i] << " ";
    cout << endl;
}

/*
    立方样条类(函数输入):
    ARGS:
        vector<double> 结点
        func& 函数
        int 边界条件
            - 0 完全立方样条
            - 1 边界二阶可导
            - 2 自然立方样条
*/
class CubicSpline_func:public SplineBase{
private:
    func& _f;
    func _zero;
    vector<double> _knot;
    int _bound_type = 0;
    vector<double> _M;
    vector<double> _s3_l;
    vector<double> _s1_l;
    vector<double> _fx_i;
    vector<double> _diff_3d;
    vector<double> _mu;
    vector<double> _phi;
    int _N;
    double _a;
    double _b;
    bool _is_illegal;

    double _2d_diff(double x, double y) {return (_f(x)-_f(y))/(x-y);}
    
    void _generate_fx_i(){
        for(int i=0;i<_N;i++) _fx_i.push_back(_f(_knot[i]));
    }
    
    void _generate_diff(){
        vector<double> _X;
        _X.push_back(_knot[0]);
        for(int i=0;i<_knot.size();i++) _X.push_back(_knot[i]);
        _X.push_back(_knot[_knot.size()-1]);
        
        for(int i=1;i<_X.size()-1;i++){
            double a = _X[i-1]; double b = _X[i]; double c = _X[i+1];
            double div1; double div2;
            if(a==b) div1 = _f.diff(a);
            else div1 = (_f(a)-_f(b))/(a-b);
            if(c==b) div2 = _f.diff(b);
            else div2 = (_f(b)-_f(c))/(b-c);
            _diff_3d.push_back((div2-div1)/(c-a));
        }
    }

    void _generate_mp(){
        _mu.push_back(0); _phi.push_back(0);
        for(int i=2;i<=_N-1;i++){
            int _index = i-1;
            _mu.push_back((_knot[_index]-_knot[_index-1])/(_knot[_index+1]-_knot[_index-1]));
            _phi.push_back((_knot[_index+1]-_knot[_index])/(_knot[_index+1]-_knot[_index-1]));
        }
        //vector_print(_mu);
        //vector_print(_phi);
    }

    void _generate_M(){
        vector<double> _A;
        vector<double> _b;
        if(_bound_type == 0){
            for(int i=1;i<=_N;i++) _b.push_back(6*_diff_3d[i-1]);
            for(int i=1;i<=_N;i++){
                for(int j=1;j<=_N;j++){
                    if(i==1){
                        if(j==i) _A.push_back(2);
                        else if(j==i+1) _A.push_back(1);
                        else _A.push_back(0);
                    }else if(i==_N){
                        if(j==i) _A.push_back(2);
                        else if(j==i-1) _A.push_back(1);
                        else _A.push_back(0);
                    }else{
                        if(j==i) _A.push_back(2);
                        else if(j==i-1) _A.push_back(_mu[i-1]);
                        else if(j==i+1) _A.push_back(_phi[i-1]);
                        else _A.push_back(0);
                    }
                    }
                }
            }
        else{
            for(int i=2;i<=_N-1;i++) _b.push_back(6*_diff_3d[i-1]);
            _b[0]-=_mu[1]*_M[0]; _b[_b.size()-1]-=_phi[_phi.size()-1]*_M[1];
            for(int i=2;i<=_N-1;i++){
                for(int j=2;j<=_N-1;j++){
                    if(i==2){
                        if(j==i) _A.push_back(2);
                        else if(j==i+1) _A.push_back(_phi[i-1]);
                        else _A.push_back(0);
                    }else if(i==_N-1){
                        if(j==i) _A.push_back(2);
                        else if(j==i-1) _A.push_back(_mu[i-1]);
                        else _A.push_back(0);
                    }else{
                        if(j==i) _A.push_back(2);
                        else if(j==i-1) _A.push_back(_mu[i-1]);
                        else if(j==i+1) _A.push_back(_phi[i-1]);
                        else _A.push_back(0);
                    }
                }
            }
        }
        //vector_print(_A);
        //vector_print(_b);
        if(_bound_type == 0){
            MatrixXd A(_N,_N);
            for(int i=0;i<_N;i++) for(int j=0;j<_N;j++) A(i,j) = _A[i*_N+j];
            MatrixXd b(_N,1);
            for(int i=0;i<_N;i++) b(i,0) = _b[i];
            VectorXd _solve = A.lu().solve(b);
            for(int i=0;i<_N;i++) _M.push_back(_solve[i]);
        }else{
            MatrixXd A(_N-2,_N-2);
            for(int i=0;i<_N-2;i++) for(int j=0;j<_N-2;j++) A(i,j) = _A[i*(_N-2)+j];
            MatrixXd b(_N-2,1);
            for(int i=0;i<_N-2;i++) b(i,0) = _b[i];
            VectorXd _solve = A.lu().solve(b);
            double temp = _M[_M.size()-1]; _M.pop_back();
            for(int i=0;i<_N-2;i++) _M.push_back(_solve[i]);
            _M.push_back(temp);
        }
    }

    void _generate_s1_diff(){
        for(int i=1;i<_N;i++){
            _s1_l.push_back(_2d_diff(_knot[i-1],_knot[i])-(_M[i]+_M[i-1]*2)*(_knot[i]-_knot[i-1])/6);
        }
    }

    void _generate_s3_diff(){
        for(int i=1;i<_N;i++){
            _s3_l.push_back((_M[i]-_M[i-1])/(_knot[i]-_knot[i-1]));
        }
    }
public:

    CubicSpline_func():_f(_zero),
                       _is_illegal(true)
    {}
    CubicSpline_func(vector<double> knot, func& f, int style):_f(f),
                                                              _knot(knot),
                                                              _bound_type(style),
                                                              _N(_knot.size()),
                                                              _a(_knot[0]),
                                                              _b(_knot[_knot.size()-1]),
                                                              _is_illegal(false)
    {
        //vector_print(_knot);
        if(!_is_increasing(_knot)){
            cout << "Input knots are not strictly increasing, generating splines fails." <<endl;
            _is_illegal = true;
            return;
        }
        if(style < 0 || style >2){
            cout << "Input an illegal bound type, set as default(0)." << endl;
            _bound_type = 0;
        }
        if(_bound_type == 1){
            _M.push_back(_f.deriv(_knot[0],2));
            _M.push_back(_f.deriv(_knot[_knot.size()-1],2));
        }else if(_bound_type == 2){
            _M.push_back(0);
            _M.push_back(0);
        }
        _generate_mp();
        _generate_diff();
        _generate_M();
        _generate_s1_diff();
        _generate_s3_diff();
        _generate_fx_i();
        //vector_print(_M);
    }

    ~CubicSpline_func()
    {
        _knot.clear();
        _M.clear();
        _s3_l.clear();
        _s1_l.clear();
        _fx_i.clear();
        _diff_3d.clear();
        _mu.clear();
        _phi.clear();
    }

    double operator () (double x){
        if(_is_illegal){
            cout << "Generating splines failed! Default output is settled to 0." << endl;
            return 0; 
        }
        int p = _ind_poly(x, _knot); double x_i = _knot[p];
        //cout << p << " " << x_i << " ";
        double ans = 0;
        ans += _s3_l[p]/6; ans *= (x-x_i);
        //cout << ans << " ";
        ans += _M[p]/2; ans *= (x-x_i);
        //cout << ans << " ";
        ans += _s1_l[p]; ans *= (x-x_i);
        //cout << ans << " ";
        ans += _fx_i[p];
        //cout << ans << endl;
        return ans;
    }

    void copy(CubicSpline_func target){
        _knot = target._knot;
        _bound_type = target._bound_type;
        _M = target._M;
        _s3_l = target._s3_l;
        _s1_l = target._s1_l;
        _fx_i = target._fx_i;
        _diff_3d = target._diff_3d;
        _mu = target._mu;
        _phi = target._phi;
        _N = target._N;
        _a = target._a;
        _b = target._b;
        _is_illegal = target._is_illegal;
    }

    /*
    void check(){
        vector_print(_knot);
        vector_print(_M);
        vector_print(_s3_l);
        vector_print(_s1_l);
        vector_print(_diff_3d);
        vector_print(_mu);
        vector_print(_phi); 
        f();   
        cout << endl;    
    }

    void f(){
        for(int i=0;i<_knot.size();i++) cout << _f(_knot[i]) << " ";
        cout <<endl;
    }*/
};
/*
    特殊映射函数类：用于散点函数创建
    ARGS:
        vector<double> 结点
        vector<double> 曲线赋值
*/
class _map:public func{
private:
    vector<double> _knot;
    vector<double> _value;
public:
    _map(vector<double> knot, vector<double> value):_knot(knot),
                                                    _value(value)
    {}

    ~_map(){
        _knot.clear();
        _value.clear();
    }

    double operator () (double x){
        for(int i=0;i<_knot.size();i++) if(_knot[i]==x) return _value[i];
        return 0;
    }

    double diff(double x){
        return 0;
    }
};
/*
    立方样条类：基于CubicSpline_func类的散点插值
    ARGS:
        vector<double> 结点
        vector<double> 函数值
*/
class CubicSpline_dots:public SplineBase{
private:
    CubicSpline_func _spline;
    vector<double> _knot;
    bool _is_illegal;

public:
    CubicSpline_dots():_is_illegal(true){}

    CubicSpline_dots(vector<double> knot, vector<double> value):_knot(knot),
                                                                _is_illegal(false)
    {
        if(!_is_increasing(_knot)){
            cout << "Input knots are not strictly increasing, generating splines fails." <<endl;
            _is_illegal = true;
            return;
        }
        _map _map(knot, value);
        CubicSpline_func _target(knot, _map, 2);
        //_target.check();
        _spline.copy(_target);
        //_spline.check();
    }

    ~CubicSpline_dots()
    {
        _knot.clear();
    }

    double operator () (double x){
        if(_is_illegal){
            cout << "Generating splines failed! Default output is settled to 0." << endl;
            return 0; 
        }
        return _spline(x);
    }

    void copy(CubicSpline_dots target){
        _spline.copy(target._spline);
        _is_illegal = target._is_illegal;
    }
};
/*
    点类：
        用于曲线拟合的返回
*/
class Point{
private:
    double _x;
    double _y;
public:
    Point():_x(0),_y(0){}
    Point(double x, double y):_x(x),_y(y){}
    ~Point(){}
    double x(){return _x;}
    double y(){return _y;}
};
/*
    立方样条类，二维曲线拟合：
    ARGS:
        vector<double> x;
        vector<double> y;
*/
class CubicSpline_2dcurve{
private:
    vector<double> _knot;
    CubicSpline_dots _X;
    CubicSpline_dots _Y;
    bool _is_illegal;
    double _dis(double x1, double y1, double x2, double y2){
        return sqrt((x1-x2)*(x1-x2)+(y1-y2)*(y1-y2));
    }
public:
    CubicSpline_2dcurve(vector<double> x, vector<double> y):_is_illegal(false)
    {
        if(x.size()!= y.size()){
            cout << "Input not a curve!" << endl;
            _is_illegal = true;
            return;
        }
        double ans = 0;
        for(int i=0;i<x.size()-1;i++){
            _knot.push_back(ans);
            ans += _dis(x[i],y[i],x[i+1],y[i+1]);
        }
        _knot.push_back(ans);
        for(int i=0;i<_knot.size();i++) _knot[i] /= ans;
        _X.copy(CubicSpline_dots(_knot, x));
        _Y.copy(CubicSpline_dots(_knot, y));
    }

    ~CubicSpline_2dcurve()
    {
        _knot.clear();
    }

    Point operator()(double t){
        if(_is_illegal){
            cout << "Generating splines failed!" << endl;
            return Point(0,0);
        }
        return Point(_X(t),_Y(t));
    }
};
/*
    B样条局部函数
*/
double B_iZ2(double x, int i){
    if(x>i-1 && x<=i) return (x-i+1)*(x-i+1)/2;
    else if(x>i && x<=i+1) return 0.75-(x-(i+0.5))*(x-(i+0.5));
    else if(x>i+1 && x<=i+2) return (i+2-x)*(i+2-x)/2;
    else return 0;
}

double B_iZ3(double x, int i){
    //cout << x << " " << endl;
    if(x>i-1 && x<=i) return (x-i+1)*(x-i+1)*(x-i+1)/6;
    else if(x>i && x<=i+1) return 2.0/3-(x-i+1)*(i+1-x)*(i+1-x)/2;
    else if(x>i+1 && x<i+3) return B_iZ3(2*i+2-x,i);
    else return 0;
}

class BSpline_cubic_func:public SplineBase{
private:
    vector<double> _a;
    int _N;
public:
    BSpline_cubic_func(int N, func& F):_N(N)
    {
        vector<double> b;
        for(int i=0;i<N;i++) b.push_back(6*F(i+1));
        b[0] = 3*F(1)+F.diff(1);
        b[b.size()-1] = 3*F(N)-F.diff(N);
        vector<double> M;
        for(int i=0;i<N;i++) for(int j=0;j<N;j++){
            if(i==0){
                if(j==i) M.push_back(2);
                else if(j==i+1) M.push_back(1);
                else M.push_back(0);
            }else if(i==N-1){
                if(j==i) M.push_back(2);
                else if(j==i-1) M.push_back(1);
                else M.push_back(0);
            }else{
                if(j==i) M.push_back(4);
                else if(j==i+1) M.push_back(1);
                else if(j==i-1) M.push_back(1);
                else M.push_back(0);
            }
        } 
        MatrixXd _M(N,N);
        for(int i=0;i<N;i++) for(int j=0;j<N;j++) _M(i,j) = M[i*_N+j];
        MatrixXd _b(N,1);
        for(int i=0;i<N;i++) _b(i,0) = b[i];
        VectorXd _solve = _M.lu().solve(_b);
        for(int i=0;i<_N;i++) _a.push_back(_solve[i]);
        _a.push_back(_a[N-2]+2*F.diff(N));
        _a.push_back(_a[1]-2*F.diff(1));
        //vector_print(_a);
    }

    double operator () (double x){
        double ans = 0; int p = int(x)-4;
        for(int i=(p>=-1?p:-1);i<=(p+6<=_N?p+6:_N);i++){
            //cout << i;
            if(i==-1) ans += _a[_N+1]*B_iZ3(x,i);
            else ans += _a[i]*B_iZ3(x,i);
        }
        return ans;
    }
};

class BSpline_quad_func:public SplineBase{
private:
    vector<double> _a;
    int _N;
public:
    BSpline_quad_func(int N, func& F):_N(N)
    {
        vector<double> b;
        for(int i=1;i<N;i++) b.push_back(8*F(i+0.5));
        b[0] -= 2*F(1);
        b[b.size()-1] -= 2*F(N);
        vector<double> M;
        for(int i=1;i<N;i++) for(int j=1;j<N;j++){
            if(i==1){
                if(j==i) M.push_back(5);
                else if(j==i+1) M.push_back(1);
                else M.push_back(0);
            }else if(i==N-1){
                if(j==i) M.push_back(5);
                else if(j==i-1) M.push_back(1);
                else M.push_back(0);
            }else{
                if(j==i) M.push_back(6);
                else if(j==i+1) M.push_back(1);
                else if(j==i-1) M.push_back(1);
                else M.push_back(0);
            }
        } 
        MatrixXd _M(N-1,N-1);
        for(int i=0;i<N-1;i++) for(int j=0;j<N-1;j++) _M(i,j) = M[i*(_N-1)+j];
        MatrixXd _b(N-1,1);
        for(int i=0;i<N-1;i++) _b(i,0) = b[i];
        VectorXd _solve = _M.lu().solve(_b);
        _a.push_back(0);
        for(int i=0;i<_N-1;i++) _a.push_back(_solve[i]);
        _a.push_back(-_a[N-1]+2*F(N));
        _a[0] = -_a[1]+2*F(1);
        //vector_print(_a);
    }

    double operator () (double x){
        double ans = 0; int p = int(x)-3;
        for(int i=(p>=0?p:0);i<=(p+6<=_N?p+6:_N);i++){
            //cout << i << endl;
            ans += _a[i]*B_iZ2(x,i);
        }
        return ans;
    }
};

class Linear_Spline:public SplineBase{
private:
    vector<double> _knots;
    vector<double> _value;
    bool _is_illegal;
public:
    Linear_Spline(vector<double> knots, vector<double> values):_knots(knots),
                                                               _value(values),
                                                               _is_illegal(false)
    {
        if(!_is_increasing(_knots)){
            cout << "Input knots are not strictly increasing, generating splines fails." <<endl;
            _is_illegal = true;
            return;
        }
    }
    ~Linear_Spline(){}
    double operator () (double x){
        if(_is_illegal){
            cout << "Generating splines failed! Default output is settled to 0." << endl;
            return 0; 
        }
        int p = _ind_poly(x,_knots);
        return (x-_knots[p])/(_knots[p+1]-_knots[p])*(_value[p+1]-_value[p]) + _value[p];
    }
};

#endif